/** @internal
 ** @file    aibhist.c
 ** @brief   Push data down the AIB tree - MEX driver
 ** @author  Andrea Vedaldi
 **/

/* AUTORIGHTS
Copyright (C) 2007-10 Andrea Vedaldi and Brian Fulkerson

This file is part of VLFeat, available under the terms of the
GNU GPLv2, or (at your option) any later version.
*/

#include<mexutils.h>

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include<string.h>
#include<assert.h>

#include <vl/generic.h>

/* ---------------------------------------------------------------- */
/** @brief MEX driver entry point
 **/

void
mexFunction (int nout, mxArray * out[], int nin, const mxArray * in[])
{
  enum {IN_PARENTS = 0, IN_DATA, IN_OPT} ;
  enum {OUT_TREE} ;

  vl_uint32 const *parents ;
  vl_uint32 *tree ;
  double const *data ;

  int nnull = 0 ;
  int histmode = 0 ;

  vl_uint32 i, P, N ;

  /* -----------------------------------------------------------------
   *                                               Check the arguments
   * -------------------------------------------------------------- */

  if ((nin < 2) || (nin > 3)) {
    mexErrMsgTxt ("Two or three arguments required.") ;
  }

  if (nout > 1) {
    mexErrMsgTxt ("Too many output arguments.") ;
  }

  if (!vlmxIsMatrix(in[IN_DATA], -1, -1)) {
    mexErrMsgTxt ("DATA must be a matrix of DOUBLE");
  }

  if (!vlmxIsVector(in[IN_PARENTS], -1)) {
    mexErrMsgTxt ("PARENTS must be a vector") ;
  }

  if (mxGetClassID(in[IN_PARENTS]) != mxUINT32_CLASS) {
    mexErrMsgTxt ("PARENTS must be UINT32") ;
  }

  N = mxGetNumberOfElements (in[IN_DATA]) ;
  data = mxGetPr (in[IN_DATA]) ;

  P = mxGetNumberOfElements (in[IN_PARENTS]) ;
  parents = mxGetData (in[IN_PARENTS]) ;

  if (nin > 2) {
    enum {buflen = 32} ;
    char buf [buflen] ;
    if (!vlmxIsString(in[IN_OPT], -1)) {
      mexErrMsgTxt("OPT must be a string") ;
    }
    mxGetString(in[IN_OPT], buf, buflen) ;
    buf [buflen - 1] = 0 ;
    if (!uStrICmp("hist", buf)) {
      mexErrMsgTxt("OPT must be equal to 'hist'") ;
    }
    histmode = 1 ;
  }

  out[OUT_TREE] = mxCreateNumericMatrix(1, P,mxUINT32_CLASS, mxREAL) ;
  tree = mxGetData (out[OUT_TREE]) ;

  /* -----------------------------------------------------------------
   *                                                        Do the job
   * -------------------------------------------------------------- */

  {
    char buf [1024] ;
    vl_uint32 max_node = 0 ;
    vl_uint32 min_node = 0 ;
    vl_uint32 last_leaf = 0 ;
    vl_uint32 root = 0 ;

    /* exhamine parents for errors and informations */
    for (i = 0 ; i  < P ; ++i) {
      vl_uint32 node = parents [i] ;

      if ((node != 0) & (node != 1)) {
        max_node = VL_MAX (node, max_node) ;
        min_node = VL_MIN (node, min_node) ;
      }

      /* check no node points outside the tree */
      if (node > P) {
        snprintf(buf, sizeof(buf),
                 "Out of bounds link PARENTS[%d] = %u > %u", i, node, P) ;
        mexErrMsgTxt (buf) ;
      }

      /* check node points to something above him */
      if ((node != 0) & (node != 1) & (node < i)) {
        snprintf(buf, sizeof(buf),
                 "Backward link PARENTS[%d] = %u < %d", i, node, i) ;
        mexErrMsgTxt (buf) ;
      }
      if (node == 0) ++ nnull ;
    }

    /* now
     *
     * min_node = first node which is not a leaf
     * max_node = root node
     * nnull    = number of leaves pointing to the null node
     */

    last_leaf = min_node - 1 ;
    root = max_node ;

    /* process data */
    for (i = 0 ; i < N ; ++i) {
      int w = 1 ;
      vl_uint32 x = data [i] ;

      if (histmode) {
        w = x ;
        x = i ;
      }

      if ((x < 1) | (x > last_leaf)) {
        if (histmode) {
          snprintf(buf, sizeof(buf),
                   "DATA length exceeds number of AIB leaves") ;
        } else {
          snprintf(buf, sizeof(buf),
                   "DATA [%u] = %u is not a leaf", i, x) ;
        }
        mexErrMsgTxt (buf) ;
      }

      while (VL_TRUE) {
        vl_uint32 x_ = parents [x -1] ;
        /*     mexPrintf("%u : x_=%u, x=%u\n", i, x_, x) ; */
        ++ tree [x - 1] ;
        if ((x_ == x) | (x_ == 0) | (x_ == 1)) break ;
        x = x_ ;
      }
    }
  }
}
